#include "decoder.h"

#include <fstream>
#include <vector>

#include "buffer.h"

namespace lrr300 {

void readExactlyCountBytes(std::ifstream& in, char* buffer, size_t len) {
    size_t bytesRead = 0;
    while (bytesRead < len) {
        // 尝试读取剩余需要的字节数
        size_t currentRead = in.readsome(buffer + bytesRead, len - bytesRead);
        // 检查是否达到文件尾或发生错误
        if (currentRead == 0 && in.eof()) {
            std::cerr << fmt::format("Unexpected EOF reached before reading {} bytes.", len) << std::endl;
            break; // 文件结束，无法读取更多数据
        } else if (currentRead == 0 && in.fail()) {
            std::cerr << "Read error occurred." << std::endl;
            in.clear(); // 清除错误标志，避免下次读取受阻
            break;
        }
        bytesRead += currentRead; // 更新已读取的字节数
    }
    if (bytesRead != len) {
        std::cerr << "Only able to read " << bytesRead << " bytes, expected 16." << std::endl;
    }
}

std::ostream& operator<<(std::ostream& os, const Point3D& p) {
    os << fmt::format("({:.3f},{:.3f},{:.3f})", p.z, p.y, p.z);
    return os;
}

struct PointCloudHeader {
    uint32_t data_type_id;           // Data type identifier (constant)
    uint32_t frame_number;           // RADAR frame number (scan cycle counter) 0-4294967295
    uint16_t packet_number_in_frame; // Packet number in sequence for this Frame 0~15
    uint16_t total_number_in_frame;  // Total packets expected for this Frame 1~16
    uint16_t target_validnum;        // targets in the  packet 1~64
    uint32_t checksum;               // 单帧校验和
};

// 0x7A027A02
struct PointCloudData {
    uint16_t obj_id;
    float target_pos_x;
    float target_pos_y;
    float target_vel_x;
    float target_vel_y;
    uint8_t target_cipvflag;
    float target_rcs;
    uint8_t target_objconfidence;
    uint8_t target_measstat;
    uint8_t target_objtrailstate;
    uint8_t target_dynprop;
    uint8_t target_objlifecycle;
    float target_objheight;
    uint8_t target_objtype;
    uint8_t target_probofexist;
    uint8_t target_type;
    uint32_t target_msgcnt;
    std::array<char, 9> res;
};

// 0x7A017A01
struct DetectPointData {
    uint16_t obj_id;
    double obj_distance;
    double obj_vreldistance;
    double obj_pitch;
    double obj_angle;
    double obj_power;
};

// 检查当前缓冲区是否包含目标字节序列
bool check_sequence(const std::vector<char>& buffer, const std::vector<char>& sequence) {
    if (buffer.size() < sequence.size()) {
        return false;
    }
    for (size_t i = 0; i < sequence.size(); ++i) {
        if (buffer[buffer.size() - sequence.size() + i] != sequence[i]) {
            return false;
        }
    }
    return true;
}

double radians(double degrees) { return degrees * M_PI / 180.0; }

double sind(double degrees) { return std::sin(radians(degrees)); }

bool Decoder::decode(const std::string& file, DataCallBack&& cbk) {
    std::ifstream in(file, std::ios::in | std::ios::binary);
    if (!in) {
        std::cerr << "Failed to open file." << std::endl;
        return 1;
    }

    const std::vector<char> target_sequence_1 = {0x7A, 0x01, 0x7A, 0x01};
    // const std::vector<char> target_sequence_1 = {0x7A, 0x02, 0x7A, 0x02};
    DecodedData decodedDataAll;
    decodedDataAll.frame_number = 99999;
    while (!in.eof()) {
        std::vector<char> buffer;
        size_t startPos = 0;
        char ch;
        while (in.get(ch)) {
            buffer.push_back(ch);
            // 检查缓冲区是否包含目标字节序列
            if (check_sequence(buffer, target_sequence_1)) {
                startPos = (size_t)in.tellg() - target_sequence_1.size();
                LOG(INFO) << "Found the sequence at position: " << startPos << std::endl;
                break;
            }
            // 保持缓冲区大小最多为目标字节序列大小
            if (buffer.size() > target_sequence_1.size()) {
                buffer.erase(buffer.begin());
            }
        }

        if (in.eof()) {
            LOG(INFO) << "File EOF." << std::endl;
            break;
        }

        in.seekg(startPos, std::ios::beg);

        char buf[18];
        in.read(buf, 18);
        hhfox::Buffer bufferreader(buf, 18);
        lrr300::PointCloudHeader header;
        header.data_type_id = bufferreader.read_4bytes<uint32_t>();
        header.frame_number = bufferreader.read_4bytes<uint32_t>();
        header.packet_number_in_frame = bufferreader.read_2bytes();
        header.total_number_in_frame = bufferreader.read_2bytes();
        header.target_validnum = bufferreader.read_2bytes();
        header.checksum = bufferreader.read_4bytes<uint32_t>();
        LOG(INFO) << fmt::format(
            "---> header.data_type_id = 0x{:x},header.frame_number = {} ,header.packet_number_in_frame = "
            "{},header.total_number_in_frame = "
            "{},header.target_validnum = "
            "{},header.checksum = {}",
            header.data_type_id, header.frame_number, header.packet_number_in_frame, header.total_number_in_frame,
            header.target_validnum, header.checksum);
        DecodedData decodedData;
        decodedData.frame_number = header.frame_number;
        if (header.data_type_id == 0x7a027a02) {
            for (int i = 0; i < header.target_validnum; i++) {
                // char data[48];
                // in.read(data, 48);
                // hhfox::Buffer bufferreader(data, 48);
                // PointCloudData pd;
                // pd.obj_id = bufferreader.read_2bytes();
                // memcpy(&pd.obj_id, data, 2);
                // memcpy(&pd.target_pos_x, data + 2, 4);
                // memcpy(&pd.target_pos_y, data + 6, 4);
                // memcpy(&pd.target_vel_x, data + 10, 4);
                // memcpy(&pd.target_vel_y, data + 14, 4);
                // memcpy(&pd.target_cipvflag, data + 18, 1);
                // memcpy(&pd.target_rcs, data + 19, 4);
                // memcpy(&pd.target_objconfidence, data + 23, 1);
                // memcpy(&pd.target_measstat, data + 24, 1);
                // memcpy(&pd.target_objtrailstate, data + 25, 1);
                // memcpy(&pd.target_dynprop, data + 26, 1);
                // memcpy(&pd.target_objlifecycle, data + 27, 1);
                // memcpy(&pd.target_objheight, data + 28, 4);
                // memcpy(&pd.target_objtype, data + 32, 1);
                // memcpy(&pd.target_probofexist, data + 33, 1);
                // memcpy(&pd.target_type, data + 34, 1);
                // memcpy(&pd.target_msgcnt, data + 35, 4);
                // LOG(INFO) << "seq:" << i << '\n' << Json(pd).dump(4);
            }
        } else if (header.data_type_id == 0x7a017a01) {
            for (int i = 0; i < header.target_validnum; i++) {
                char data[16];
                in.read(data, 16);
                hhfox::Buffer bufferreader(data, 16);
                DetectPointData dd;
                dd.obj_id = bufferreader.read_2bytes();
                dd.obj_distance = bufferreader.read_4bytes<uint32_t>() * 0.01;
                dd.obj_vreldistance = bufferreader.read_4bytes<int32_t>() * 0.001;
                dd.obj_pitch = bufferreader.read_2bytes() * 0.01;
                dd.obj_angle = bufferreader.read_2bytes() * 0.01;
                dd.obj_power = bufferreader.read_2bytes() * 0.1;
                Point3D point;
                point.x = dd.obj_distance * sind(dd.obj_angle);
                point.z = dd.obj_distance * sind(dd.obj_pitch);
                point.y = sqrt(dd.obj_distance * dd.obj_distance - point.x * point.x - point.z * point.z);
                // LOG(INFO) << fmt::format(
                //     "v.obj_id = {},v.obj_distance = {:.2f} ,v.obj_vreldistance = {:.3f},v.obj_angle = "
                //     "{:.2f},v.obj_pitch = "
                //     "{:.2f},v.obj_power = {:.2f}",
                //     dd.obj_id, dd.obj_distance, dd.obj_vreldistance, dd.obj_angle, dd.obj_pitch, dd.obj_power);
                // LOG(INFO) << fmt::format("x = {:.3f},y = {:.3f}, z = {:.3f}", point.x, point.y, point.z);
                // decodedData.data.push_back(point);
                decodedDataAll.data.push_back(std::move(point));
            }
        }
        // cbk(decodedData);
    }
    cbk(decodedDataAll);
    return true;
}

bool Decoder::decode_single_frame(const std::string& file, DataCallBack&& cbk) {
    std::ifstream in(file, std::ios::in | std::ios::binary);
    if (!in) {
        std::cerr << "Failed to open file." << std::endl;
        return 1;
    }
    bool full_frame = false;
    const std::vector<char> target_sequence_1 = {0x7A, 0x01, 0x7A, 0x01};
    // const std::vector<char> target_sequence_1 = {0x7A, 0x02, 0x7A, 0x02};
    while (!in.eof()) {
        std::vector<char> buffer;
        size_t startPos = 0;
        char ch;
        while (in.get(ch)) {
            buffer.push_back(ch);
            // 检查缓冲区是否包含目标字节序列
            if (check_sequence(buffer, target_sequence_1)) {
                startPos = (size_t)in.tellg() - target_sequence_1.size();
                LOG(INFO) << "Found the sequence at position: " << fmt::format("{:08x}", startPos) << std::endl;
                break;
            }
            // // 保持缓冲区大小最多为目标字节序列大小
            // if (buffer.size() > target_sequence_1.size()) {
            //     buffer.erase(buffer.begin());
            // }
        }

        if (in.eof()) {
            LOG(INFO) << "File EOF." << std::endl;
            break;
        }

        in.seekg(startPos, std::ios::beg);

        char buf[18];
        // in.read(buf, 18);
        readExactlyCountBytes(in, buf, 18);
        hhfox::Buffer bufferreader(buf, 18);
        lrr300::PointCloudHeader header;
        header.data_type_id = bufferreader.read_4bytes<uint32_t>();
        header.frame_number = bufferreader.read_4bytes<uint32_t>();
        header.packet_number_in_frame = bufferreader.read_2bytes();
        header.total_number_in_frame = bufferreader.read_2bytes();
        header.target_validnum = bufferreader.read_2bytes();
        header.checksum = bufferreader.read_4bytes<uint32_t>();
        LOG(INFO) << fmt::format(
            "---> header.data_type_id = 0x{:x},header.frame_number = {} ,header.packet_number_in_frame = "
            "{},header.total_number_in_frame = "
            "{},header.target_validnum = "
            "{},header.checksum = {}",
            header.data_type_id, header.frame_number, header.packet_number_in_frame, header.total_number_in_frame,
            header.target_validnum, header.checksum);
        static DecodedData decodedData;
        decodedData.frame_number = header.frame_number;
        if (header.data_type_id == 0x7a017a01) {
            for (int i = 0; i < header.target_validnum; i++) {
                char data[16];
                // in.read(data, 16);
                readExactlyCountBytes(in, data, 16);
                hhfox::Buffer bufferreader(data, 16);
                DetectPointData dd;
                dd.obj_id = bufferreader.read_2bytes();
                dd.obj_distance = bufferreader.read_4bytes<uint32_t>() * 0.01;
                dd.obj_vreldistance = bufferreader.read_4bytes<int32_t>() * 0.001;
                dd.obj_pitch = bufferreader.read_2bytes() * 0.01;
                dd.obj_angle = bufferreader.read_2bytes() * 0.01;
                dd.obj_power = bufferreader.read_2bytes() * 0.1;
                LOG(INFO) << fmt::format(
                    "v.obj_id = {},v.obj_distance = {:.2f} ,v.obj_vreldistance = {:.3f},v.obj_angle = "
                    "{:.2f},v.obj_pitch = "
                    "{:.2f},v.obj_power = {:.2f}",
                    dd.obj_id, dd.obj_distance, dd.obj_vreldistance, dd.obj_angle, dd.obj_pitch, dd.obj_power);
                Point3D point;
                point.x = dd.obj_distance * sind(dd.obj_angle);
                point.z = dd.obj_distance * sind(dd.obj_pitch);
                point.y = sqrt(dd.obj_distance * dd.obj_distance - point.x * point.x - point.z * point.z);
                LOG(INFO) << fmt::format("x = {:.3f},y = {:.3f}, z = {:.3f}", point.x, point.y, point.z);
                decodedData.data.push_back(point);
                if (header.packet_number_in_frame == header.total_number_in_frame - 1) {
                    full_frame = true;
                }
            }
        }
        if (full_frame) {
            cbk(decodedData);
            full_frame = false;
            decodedData.clear();
        }
    }
    return true;
}

bool Decoder::decode_single_frame_v2(const std::string& plyfile, DataCallBack2&& cbk) {
    std::ifstream in(plyfile, std::ios::in);
    std::string line;
    bool isfirst = true;
    DecodedData2 data;
    while (std::getline(in, line)) {
        static int index = 0;
        if (line == "ply" && !isfirst) {
            cbk(data);
            data.data.clear();
            data.data.push_back(line);
            data.frame_number = ++index;
        } else {
            data.data.push_back(line);
            isfirst = false;
        }
    }
    return 0;
}

} // namespace lrr300